#include "Functions.h"
#include "FloatArithmetic.h"

#define messOn

void IConvolution(int* convolution, sbyte* convolution_o, int* spectrum, sbyte* spectrum_o);
byte IFFT(bytep signal, int* spectr, sbyte* spectr_o);
void IGetHerz(int* freq, sbytep freq_o);	

void Convolution(float* convolution, float* spectrum, float* windows_Points);
void MelPoints(float* window_Points);
byte FFT(bytep Signal, float * Spectr);
float GetMels(float freq);
float GetHerz(float freq);	

void DCT(float* convolution, float* dct);

#define MELSCALE_ORDER -6
#define MELSCALE_DELTA 50789978
static const int MelScalePoints[] = { 0, 50789978, 101579956, 152369934, 203159912,
										253949890, 304739868, 355529846, 406319824,
										457109802, 507899780, 558689758, 609479736,
										660269714, 711059692, 761849670, 812639648,
										863429626, 914219604, 965009583		
									};

#define INTEGER_CONSTANT 700

#define FLOAT_CONSTANT_MANTISSA 112701048
#define FLOAT_CONSTANT_ORDER -5

#if FURIE_MEMBERS_MAX == 4096
	#define FREQ_STEP_MANTISSA 299036121
	#define FREQ_STEP_ORDER -9
#endif

#if FURIE_MEMBERS_MAX == 2048
	#define FREQ_STEP_MANTISSA 149527979
	#define FREQ_STEP_ORDER -9
#endif

#if FURIE_MEMBERS_MAX == 1024
	#define FREQ_STEP_MANTISSA 747664692
	#define FREQ_STEP_ORDER -10
#endif

//Real part coefficients' mantissa and orders
//-1.0000000000000000F,  0.0000000000000000F,  0.7071067811865475F,
//0.9238795325112867F,  0.9807852804032304F,  0.9951847266721969F,
//0.9987954562051724F,  0.9996988186962042F,  0.9999247018391445F,
//0.9999811752826011F,  0.9999952938095761F,  0.9999988234517018F,
//0.9999997058628822F,  0.9999999264657178F
static const int Rcoef[14] = { -1, 0, 707106781, 923879532, 980785280, 995184726,
								998795456, 999698818, 999924701, 999981175, 999995294,
								999998823, 999999706, 999999926
							 };	

static const sbyte Rcoef_o[14] = { 0, 0, -9, -9, -9, -9, -9, -9, -9, -9, -9, -9, -9, -9 };

//Imaginary part coefficients' mantissa and orders
//0.0000000000000000F, -1.0000000000000000F, -0.7071067811865474F,
//-0.3826834323650897F, -0.1950903220161282F, -0.0980171403295606F,
//-0.0490676743274180F, -0.0245412285229122F, -0.0122715382857199F,
//-0.0061358846491544F, -0.0030679567629659F, -0.0015339801862847F,
//-0.0007669903187427F, -0.0003834951875714F
static const int Icoef[14] = { 0, -1, -707106781, -382683432, -195090322, -980171403, 
								-490676743, -245412285, -122715382, -613588464, -306795676, 
								-153398018, -766990318, -383495187
							 };

static const sbyte Icoef_o[14] = { 0, 0, -9, -9, -9, -10, -10, -10, -10, -11, -11, -11, -12, -12 };

void IProcessSignal(bytep signal, uint size, int* result, sbyte* result_o, uint* parts)
{
	int _signal_part_spectr[FURIE_MEMBERS_MAX];
	sbyte _signal_part_spectr_o[FURIE_MEMBERS_MAX];
	
	register byte _not_complete = size % FURIE_MEMBERS_MAX;
	register uint _parts = (_not_complete) ? size / FURIE_MEMBERS_MAX + 1 : size / FURIE_MEMBERS_MAX;

	register int* result_pnt = result;
	register sbyte* result_o_pnt = result_o;

	uint i = 0;

	for(i = 0; i < _parts; i++)
	{
		if(i == (_parts - 1) && (_not_complete))
		{
			_parts--;
		//	Furie(_signal_part_spectr, signal, _not_complete);
			break;
		}
		else
			IFFT(signal, _signal_part_spectr, _signal_part_spectr_o);

		IConvolution(result_pnt, result_o_pnt, _signal_part_spectr, _signal_part_spectr_o);

		result_pnt += WINDOW_COUNT;
		result_o_pnt += WINDOW_COUNT;
		signal += FURIE_MEMBERS_MAX;
	}

	*parts = _parts;
}


byte IFFT(bytep signal, int* spectr, sbyte* spectr_o)
{
	register int  i, j, n, k, io, _count, _count_next, _half_count;

	int ru, iu, rtp, itp, rtq, itq, rw, iw, sr;
	sbyte ru_o, iu_o, rtp_o, itp_o, rtq_o, itq_o, rw_o, iw_o, sr_o;
	
	int Rdat[FURIE_MEMBERS_MAX];
	sbyte Rdat_o[FURIE_MEMBERS_MAX];

	int Idat[FURIE_MEMBERS_MAX];
	sbyte Idat_o[FURIE_MEMBERS_MAX];

	for(i = 0; i < FURIE_MEMBERS_MAX; i++)
	{
		Rdat[i] = signal[i];
		Rdat_o[i] = 0;

		Idat[i] = 0;
		Idat_o[i] = 0;
	}
	
	_half_count = FURIE_MEMBERS_MAX >> 1;
	_count = FURIE_MEMBERS_MAX;

	for(n = 1; n <= LOG_FURIE_MEMBERS_MAX; n++)
	{
		rw = Rcoef[LOG_FURIE_MEMBERS_MAX - n];
		rw_o = Rcoef_o[LOG_FURIE_MEMBERS_MAX - n];
	
		iw = Icoef[LOG_FURIE_MEMBERS_MAX - n];
		iw_o = Icoef_o[LOG_FURIE_MEMBERS_MAX - n];

		_count_next = _count >> 1;
	
		ru = 1;
		ru_o = 0;
		iu = 0;
		iu_o = 0;

		for(j=0; j<_count_next; j++)
		{
			for(i = j; i < FURIE_MEMBERS_MAX; i += _count)
			{
				io = i + _count_next;
		
				ISum(Rdat[i], Rdat_o[i], Rdat[io], Rdat_o[io], &rtp, &rtp_o);		//rtp = Rdat[i] + Rdat[io];
				ISum(Idat[i], Idat_o[i], Idat[io], Idat_o[io], &itp, &itp_o);		//itp = Idat[i] + Idat[io];
				itp = 1;
				itp_o = 0;

				ISub(Rdat[i], Rdat_o[i], Rdat[io], Rdat_o[io], &rtq, &rtq_o);		//rtq = Rdat[i] - Rdat[io];
				ISub(Idat[i], Idat_o[i], Idat[io], Idat_o[io], &itq, &itq_o);		//itq = Idat[i] - Idat[io];
				itq = 1;
				itq_o = 0;

				Rdat[i]  = rtp;
				Rdat_o[i]  = rtp_o;
				Idat[i]  = itp;
				Idat_o[i]  = itp_o;

				IMult(rtq, rtq_o, ru, ru_o, &rtp, &rtp_o);
				IMult(itq, itq_o, iu, iu_o, &itp, &itp_o);
				ISub(rtp, rtp_o, itp, itp_o, &Rdat[io], &Rdat_o[io]);				//Rdat[io] = rtq * ru - itq * iu;

				IMult(rtq, rtq_o, iu, iu_o, &rtp, &rtp_o);
				IMult(itq, itq_o, ru, ru_o, &itp, &itp_o);
				ISum(rtp, rtp_o, itp, itp_o, &Idat[io], &Idat_o[io]);				//Idat[io] = itq * ru + rtq * iu;
			}
			
			sr = ru;
			sr_o = ru_o;

			IMult(ru, ru_o, rw, rw_o, &rtp, &rtp_o);
			IMult(iu, iu_o, iw, iw_o, &itp, &itp_o);
			ISub(rtp, rtp_o, itp, itp_o, &ru, &ru_o);								//ru = rw * ru - iw * iu;

			IMult(iu, iu_o, rw, rw_o, &rtp, &rtp_o);
			IMult(sr, sr_o, iw, iw_o, &itp, &itp_o);
			ISum(rtp, rtp_o, itp, itp_o, &iu, &iu_o);								//iu = rw * iu + iw * sr;
		}
			
		_count >>= 1;
	}
 
	for(j = i = 1; i < FURIE_MEMBERS_MAX; i++)
	{
		if(i < j)
		{
			io = i - 1;
			_count_next = j - 1;

			rtp = Rdat[_count_next];
			rtp_o = Rdat_o[_count_next];
			itp = Idat[_count_next];
			itp_o = Idat_o[_count_next];

			Rdat[_count_next] = Rdat[io];
			Rdat_o[_count_next] = Rdat_o[io];
			Idat[_count_next] = Idat[io];
			Idat_o[_count_next] = Idat_o[io];

			Rdat[io] = rtp;
			Rdat_o[io] = rtp_o;
			Idat[io] = itp;
			Idat_o[io] = itp_o;
		}
 
		k = _half_count;
 
		while(k < j)
		{
			j = j - k;
			k >>= 1;
		}

		j = j + k;
	}
	
	for(i = 0; i< FURIE_MEMBERS_MAX; i++)
	{
		ISqr(&Rdat[i], &Rdat_o[i]);
		ISum(Rdat[i], Rdat_o[i], 0, 0, &spectr[i], &spectr_o[i]);
	}

	return true;
}

void IConvolution(int* convolution, sbyte* convolution_o, int* spectrum, sbyte* spectrum_o)
{
	register uint i = 0, j = 0;
	
	register int pre_point = 0, post_point = 0, central_point = 0;

	register byte lager_post = 0, lager_pre = 0, lager_central = 0; 

	int freq_j = 0;
	sbyte freq_j_o = 0;

	int window = 0;
	sbyte window_o = 0;

	pre_point = MelScalePoints[0];
	central_point = MelScalePoints[1];
	
	for(i = 1; i < WINDOW_COUNT - 1; i++)
	{
		post_point = MelScalePoints[i + 1];

		freq_j = 0;
		freq_j_o = 0;

		convolution[i-1] = 0;
		convolution_o[i-1] = 0;

		for(j = 0; j < FURIE_MEMBERS_MAX; j++)
		{
			lager_post = ILager(freq_j, freq_j_o, post_point, MELSCALE_ORDER);
			lager_central = ILager(freq_j, freq_j_o, central_point, MELSCALE_ORDER);
			lager_pre = ILager(freq_j, freq_j_o, pre_point, MELSCALE_ORDER);

			if(!lager_pre)
			{
				window = 0;
				window_o = 0;

				ISum(freq_j, freq_j_o, FREQ_STEP_MANTISSA, FREQ_STEP_ORDER, &freq_j, &freq_j_o);		//freq_j += freq_j_step;

				continue;
			}

			if(lager_pre && !lager_central)
			{
				ISub(freq_j, freq_j_o, pre_point, MELSCALE_ORDER, &window, &window_o);					//window = freq_j - pre_point
				IDiv(window, window_o, MELSCALE_DELTA, MELSCALE_ORDER, &window, &window_o);				//window = (freq_j - pre_point)/delta_pre;
			}

			if(lager_central && !lager_post)
			{
				ISub(post_point, MELSCALE_ORDER, freq_j, freq_j_o, &window, &window_o);					//window = post_point - freq_j
				IDiv(window, window_o, MELSCALE_DELTA, MELSCALE_ORDER, &window, &window_o);				//window = (post_point - freq_j)/delta_post;
			}

			if(lager_post)
				break;
			
			IGetHerz(&window, &window_o);
			IMult(window, window_o, spectrum[j], spectrum_o[j], &window, &window_o);
			ISum(window, window_o, convolution[i-1], convolution_o[i-1], &convolution[i-1], &convolution_o[i-1]);	//convolution[i-1] += spectrum[j]*GetHerz(window);
		
			ISum(freq_j, freq_j_o, FREQ_STEP_MANTISSA, FREQ_STEP_ORDER, &freq_j, &freq_j_o);		//freq_j += freq_j_step;
		}

		pre_point = central_point;
		central_point = post_point;
	}
}

void IGetHerz(int* freq, sbytep freq_o)
{
	IDiv(*freq, *freq_o, FLOAT_CONSTANT_MANTISSA, FLOAT_CONSTANT_ORDER, freq, freq_o);			//freq/1127.01048
	IExp(freq, freq_o);																			//exp(freq/1127.01048) - 1
	IMult(*freq, *freq_o, INTEGER_CONSTANT, 0, freq, freq_o);									//700*(exp(freq/1127.01048) - 1)
}

void ProcessSignal(bytep signal, uint size, float* result, uint* parts)
{
	float _signal_part_spectr[FURIE_MEMBERS_MAX];
	float _melPoints[WINDOW_COUNT];
	
	byte _not_complete = size % FURIE_MEMBERS_MAX;
	uint _parts = (_not_complete) ? size / FURIE_MEMBERS_MAX + 1 : size / FURIE_MEMBERS_MAX;
	float * result_pnt = result;

	uint i = 0;

	MelPoints(_melPoints);

	for(i = 0; i < _parts; i++)
	{
		if(i == (_parts - 1) && (_not_complete))
		{
			_parts--;
		//	Furie(_signal_part_spectr, signal, _not_complete);
			break;
		}
		else
			FFT(signal, _signal_part_spectr);

		Convolution(result_pnt, _signal_part_spectr, _melPoints);

		result_pnt += WINDOW_COUNT;
		signal += FURIE_MEMBERS_MAX;
	}

	*parts = _parts;
}
 
byte FFT(bytep Signal, float * Spectr)
{
	register int  i, j, n, k, io, _count, _count_next, _half_count;
	float         ru, iu, rtp, itp, rtq, itq, rw, iw, sr;
 
	float Rdat[FURIE_MEMBERS_MAX];
	float Idat[FURIE_MEMBERS_MAX];

	static const float Rcoef[14] =
	{  
		-1.0000000000000000F,  0.0000000000000000F,  0.7071067811865475F,
		0.9238795325112867F,  0.9807852804032304F,  0.9951847266721969F,
		0.9987954562051724F,  0.9996988186962042F,  0.9999247018391445F,
		0.9999811752826011F,  0.9999952938095761F,  0.9999988234517018F,
		0.9999997058628822F,  0.9999999264657178F
	};
  
	static const float Icoef[14] =
	{  
		 0.0000000000000000F, -1.0000000000000000F, -0.7071067811865474F,
		-0.3826834323650897F, -0.1950903220161282F, -0.0980171403295606F,
		-0.0490676743274180F, -0.0245412285229122F, -0.0122715382857199F,
		-0.0061358846491544F, -0.0030679567629659F, -0.0015339801862847F,
		-0.0007669903187427F, -0.0003834951875714F
	};
 
	for(i = 0; i < FURIE_MEMBERS_MAX; i++)
	{
		Rdat[i] = Signal[i];
		Idat[i] = 0;
	}
	
	_half_count = FURIE_MEMBERS_MAX >> 1;
	_count = FURIE_MEMBERS_MAX;
	
	for(n = 1; n <= LOG_FURIE_MEMBERS_MAX; n++)
	{
		rw = Rcoef[LOG_FURIE_MEMBERS_MAX - n];
		iw = Icoef[LOG_FURIE_MEMBERS_MAX - n];

		_count_next = _count >> 1;
		ru = 1.0F;
		iu = 0.0F;

		for(j=0; j<_count_next; j++)
		{
			for(i = j; i < FURIE_MEMBERS_MAX; i += _count)
			{
				io       = i + _count_next;
				rtp      = Rdat[i]  + Rdat[io];
				itp      = Idat[i]  + Idat[io];
				rtq      = Rdat[i]  - Rdat[io];
				itq      = Idat[i]  - Idat[io];
			
				Rdat[i]  = rtp;
				Idat[i]  = itp;

				Rdat[io] = rtq * ru - itq * iu;
				Idat[io] = itq * ru + rtq * iu;
				
			}
			
			sr = ru;
			ru = ru * rw - iu * iw;
			iu = iu * rw + sr * iw;
		}
 
		_count >>= 1;
	}
 
	for(j = i = 1; i < FURIE_MEMBERS_MAX; i++)
	{
		if(i < j)
		{
			io = i - 1;
			_count_next = j - 1;
			rtp = Rdat[_count_next];
			itp = Idat[_count_next];
			Rdat[_count_next] = Rdat[io];
			Idat[_count_next] = Idat[io];
			Rdat[io] = rtp;
			Idat[io] = itp;
		}
 
		k = _half_count;
 
		while(k < j)
		{
			j   = j - k;
			k >>= 1;
		}

		j = j + k;
	}
	
	for(i = 0; i< FURIE_MEMBERS_MAX; i++)
		Spectr[i] = Rdat[i]*Rdat[i];
 
	return true;
}
 
//Form windows on mel-scale
void MelPoints(float* window_Points)
{
	uint i = 0;
	float step = 0;

	window_Points[0] = 0;
	window_Points[WINDOW_COUNT - 1] = GetMels(MAX_FREQUENCY);

	step = window_Points[WINDOW_COUNT - 1]/(float)WINDOW_COUNT;

	for(i = 1; i < WINDOW_COUNT; i++)
		window_Points[i] = window_Points[i - 1] + step;
		
}

void Convolution(float* convolution, float* spectrum, float* windows_Points)
{
	uint i = 0, j = 0;
	
	float pre_point = 0;
	float post_point = 0;
	float central_point = 0;
	float delta_post = 0;
	float delta_pre = 0;

	float freq_j = 0;
	float freq_j_step = GetMels(FURIE_MEMBERS_MAX/(float)SAMPLE_RATE);

	float window = 0;

	for(i = 1; i < WINDOW_COUNT - 1; i++)
	{
		pre_point = windows_Points[i - 1];
		post_point = windows_Points[i + 1];
		central_point = windows_Points[i];
		delta_post = post_point - central_point;
		delta_pre = central_point - pre_point;

		freq_j = 0;
		convolution[i-1] = 0;
		
		for(j = 0; j < FURIE_MEMBERS_MAX; j++)
		{
			if(freq_j <= pre_point)
			{
				window = 0;

				freq_j += freq_j_step;
				continue;
			}

			if(freq_j > pre_point && freq_j <= central_point)
				window = (freq_j - pre_point)/delta_pre;


			if(freq_j > central_point && freq_j < post_point)
				window = (post_point - freq_j)/delta_post;

			if(freq_j >= post_point)
				break;
			
			convolution[i-1] += spectrum[j]*GetHerz(window);
		
			freq_j += freq_j_step;
		}
	}
}

float GetMels(float freq)
{
	float result = 1127.01048f; //magical number with floating point, as we like
	result *= logf(1 + freq/700);
	
	return result;
}

float GetHerz(float freq)
{
	float result = 1127.01048f; //magical number with floating point, as we like;
	result = 700*(expf(freq/result) - 1);

	return result;
}

void DCT(float* convolution, float* dct)
{
	uint i = 0, j = 0;
	
	for(i = 0; i < WINDOW_COUNT - 1; i++)
		convolution[i] = logf(convolution[i]);
	
	for(i = 0; i < DCT_MEMBERS; i++)
	{
		dct[i] = 0;

		for(j = 0; j < WINDOW_COUNT - 1; j++)
			dct[i] += convolution[j]*cosf((i+1)*(j+1-0.5)*pi/WINDOW_COUNT);
	}
}

